#include "data_mocker.h"
#include "../remote_vehicle.h"
#include "../world.h"

#include "interface/msg/event.hpp"

using namespace decision::adas;

DataMocker::DataMocker(World& world, double fps)
    : AdasApplication(world, "Mock", fps)
{
    pub_ = this->create_publisher<interface::msg::Objects>("/perception/multiFused/objects", 10);
}

void DataMocker::detect()
{
    RCLCPP_INFO(logger_, "DataMocker");

    interface::msg::Objects objects;

    {
        objects.objects.emplace_back();
        auto& object = objects.objects.back();
        object.source = 1;
        object.id = 5;
        object.type = 1;
        object.x = move_x--;
        object.y = move_y -= 0.9;
        object.z = 0;
        object.heading = 30;
        object.speed = 0;
        object.length = 5;
        object.width = 2;
        object.height = 1.4;
    }

    // {
    //     objects.objects.emplace_back();
    //     auto& object = objects.objects.back();
    //     object.source = 101;
    //     object.id = 2;
    //     object.type = 1;
    //     object.x = 10;
    //     object.y = 1;
    //     object.z = 0;
    //     object.heading = 30;
    //     object.speed = 0;
    //     object.length = 5;
    //     object.width = 2;
    //     object.height = 1.4;
    // }

    // {
    //     objects.objects.emplace_back();
    //     auto& object = objects.objects.back();
    //     object.source = 101;
    //     object.id = 3;
    //     object.type = 1;
    //     object.x = 10;
    //     object.y = -10;
    //     object.z = 0;
    //     object.heading = 30;
    //     object.speed = 0;
    //     object.length = 5;
    //     object.width = 2;
    //     object.height = 1.4;
    // }

    // {
    //     objects.objects.emplace_back();
    //     auto& object = objects.objects.back();
    //     object.source = 101;
    //     object.id = 4;
    //     object.type = 1;
    //     object.x = -10;
    //     object.y = -1;
    //     object.z = 0;
    //     object.heading = 30;
    //     object.speed = 0;
    //     object.length = 5;
    //     object.width = 2;
    //     object.height = 1.4;
    // }

    // {
    //     objects.objects.emplace_back();
    //     auto& object = objects.objects.back();
    //     object.source = 101;
    //     object.id = 5;
    //     object.type = 1;
    //     object.x = -10;
    //     object.y = 1;
    //     object.z = 0;
    //     object.heading = 30;
    //     object.speed = 0;
    //     object.length = 5;
    //     object.width = 2;
    //     object.height = 1.4;
    // }

    pub_->publish(objects);
}
